//VERSION 1.0 - Flown on Mini Bexus 26. Sept 2012 to ~10km alt
/*
1 update all needed variable
2 check if reached
3 check if reachable by height
4 calculate heading
5 calculate nick
*/

// CHANGE
long _last_t = 0;
float _last_derivative = 0;
long _last_error = 0; // hundreds of degrees 

//###################### main function begin ###########################
boolean vx_navigation(long in_lat, long in_lon, long in_alt) //0 = wp not reached; 1 = wp reached; 2 = wp not reacheable (glider to low)
{
  vx_g_wpin.lat = in_lat;
  vx_g_wpin.lon = in_lon;
  vx_g_wpin.alt = in_alt;
  
  //1. Index 1 is current location, Index 2 is next waypoint - trigonometry functions need radians and return float float (4byte)
  //vx_lat1 = g_gps->latitude; //long deg*10^7
  vx_lat1 = vx_g_current_pos.lat; //long deg*10^7
  vx_lat2 = vx_g_wpin.lat; //long deg*10^7
  //vx_lon1 = g_gps->longitude; //long deg*10^7
  vx_lon1 = vx_g_current_pos.lon; //long deg*10^7
  vx_lon2 = vx_g_wpin.lon; //long deg*10^7
  
  vx_g_nav_pidlistinuse = &vx_g_nav_pidlist_nav1; 
  vx_g_stab_pidlistinuse = &vx_g_stab_pidlist_nav1;

  float scalerad = ((float)(vx_lat2)/ 10000000.) * toRad;
  scaleLongDown = cos(scalerad); //has no unit [-]
  vx_dlat    = (float)(vx_lat2 - vx_lat1); //d_long casted to float [degree]
  vx_dlon   = (float)(vx_lon2 - vx_lon1) * scaleLongDown; // d_long casted to float [degree]
  //2.
  vx_g_wpdistance = vx_calculate_wp_distance(); //in meter

  if (vx_g_wpdistance < vx_wpradius)
  {
    return true;
  }

  //3. - north = 0 or 2PI, east = PI/2; south = PI
  vx_calculate_target_heading();
  vx_nav_diff_heading = GPStoRad(vx_g_current_ground_course) - vx_g_target_heading;
  
  

  //dicision wether left or right turn
  if(vx_nav_diff_heading > PI) vx_nav_error_heading = vx_nav_diff_heading - 2*PI;
  if(vx_nav_diff_heading < -PI) vx_nav_error_heading = vx_nav_diff_heading + 2*PI;
  	else vx_nav_error_heading = vx_nav_diff_heading;
  
  // CHANGE START
  long error = RadtoDeg(vx_nav_error_heading*100.0); // <--- input
  float derivative_out = 0; // <-- output
  
  long tnow = millis();
  long dt = tnow - _last_t;
  float delta_time;
  int _fCut = 20;
  
  
  if(_last_t == 0 || dt > 1000) {
    dt = 0;
  }
  _last_t = tnow;
  
  delta_time = (float)dt / 1000.0; // In seconds (float)
  
  // Compute derivative
  if ((fabs(vx_g_nav_pidlistinuse->a_3) > 0) && (dt > 0)) {
    float derivative = (error - _last_error) / delta_time;
    
    float RC = 1/(2*M_PI*_fCut);
    derivative = _last_derivative + (delta_time / (RC + delta_time)) * (derivative - _last_derivative);
    
    // Update state
    _last_error = error;
    _last_derivative = derivative;
    
    derivative_out = derivative*toRad; // <----------- output
    // Add in the derivative component later
  }
  // CHANGE END
  
  /*
  if (vx_g_calculate_navdampening)
  {
    //damping roll
    vx_nav_errorrate_heading = vx_nav_error_heading - vx_nav_error_heading_old;
    vx_nav_error_heading_old = vx_nav_error_heading;
    
    //damping nick
    vx_nav_error_nick = ahrs.pitch - vx_target_flightpathangle;
    vx_nav_errorrate_nick = vx_nav_error_nick - vx_nav_error_nick_old;
    vx_nav_error_nick_old = vx_nav_error_nick;
    
    vx_g_calculate_navdampening = false;
  }
  */
  
  

  //limitation to max roll angle
  //vx_g_target_roll = -vx_g_nav_pidlistinuse->a_1 * vx_nav_error_heading - vx_g_nav_pidlistinuse->a_3 * vx_nav_errorrate_heading; // (*(-1)) to turn in opposite direction
  vx_g_target_roll = -vx_g_nav_pidlistinuse->a_1 * vx_nav_error_heading - vx_g_nav_pidlistinuse->a_3 * derivative_out; // (*(-1)) to turn in opposite direction // CHANGE
  if(vx_g_target_roll < -vx_g_nav_pidlistinuse->a_5) vx_g_target_roll = -vx_g_nav_pidlistinuse->a_5; //leftward limitation
  if(vx_g_target_roll > vx_g_nav_pidlistinuse->a_5) vx_g_target_roll = vx_g_nav_pidlistinuse->a_5; //rightward limitation

  // **** nick calculations 
  //4.
  vx_calculate_target_flightpathangle();
  // NOTE: nick error is already used in vx_stabilize
  vx_g_target_nick = vx_g_nav_pidlistinuse->b_1 * vx_target_flightpathangle - vx_g_nav_pidlistinuse->b_3 * vx_nav_errorrate_nick;

  //limitation to min/max nick angle
  if(vx_g_target_nick < vx_g_nav_pidlistinuse->b_6) vx_g_target_nick = vx_g_nav_pidlistinuse->b_6; //downward limitation
  if(vx_g_target_nick > vx_g_nav_pidlistinuse->b_5) vx_g_target_nick = vx_g_nav_pidlistinuse->b_5; //upward limitation

  return false;
};
//###################### main function end ###########################

//###################### calculate distance begin ###########################
long vx_calculate_wp_distance()
{
  //WP1 is current location, WP2 is next waypoint - sin and cos require radian and return float float (4byte)
  if(vx_lat1 == 0 || vx_lon1 == 0)
    return -1;
  if(vx_lat2 == 0 || vx_lon2 == 0)
    return -1;

  return sqrt( sq(vx_dlat) + sq(vx_dlon)) * 0.01113195; 

};
// Overloading function for parachute checks (didn't want to screw with the original one /steenari)
long vx_calculate_wp_distance(long lat1, long lon1, long lat2, long lon2)
{
  if(lat1 == 0 || lon1 == 0 || lat2 == 0 || lon2 == 0) {
    return -1;
  }
    
  float dlat = (float)(lat2 - lat1); //d_long casted to float [degree] 
  float dlon  = (float)(lon2 - lon1) * scaleLongDown; // d_long casted to float [degree]

  return (long) sqrt( sq(dlat) + sq(dlon)) * 0.01113195;

};
//###################### calculate distance end ###########################

//###################### calculate heading begin ###########################
void vx_calculate_target_heading()
{
  if(vx_g_use_fixed_target_heading) {
    if((millis() - vx_g_use_fixed_target_heading_timer) < 60000) {
      vx_g_target_heading = vx_g_fixed_target_heading;
    }
    else {
      vx_g_use_fixed_target_heading = false;
      vx_teleSendDescent();
    }
  }
  else {
    // Heading angle is counter-clockwise from positive y-axis (flip atan2 arguments)
    vx_nav_course_heading = atan2(vx_dlon, vx_dlat);
    
    // atan2 returns from: -PI to +PI --- transform to from: 0 to +2*PI
    if(vx_nav_course_heading < 0) {
      vx_nav_course_heading = vx_nav_course_heading + TWO_PI;
    }
    
    // Double check so it gives 0 instead of TWO_PI
    if(vx_nav_course_heading == TWO_PI) {
      vx_nav_course_heading = 0.0;
    }
  
    vx_g_target_heading = vx_nav_course_heading;
  }
};


//###################### calculate heading end ###########################

//###################### calculate nick begin ###########################
void vx_calculate_target_flightpathangle()
{ 
  // *** Manual altitude control
  //vx_g_target_nick = vx_g_rcin_nick() * vx_max_nick_angle_fbw  * 0.0001745; //fbw target nick
  
  // *** GPS controlled altitude
  // dalt is positive for wp's above current position (and negative for points below)
  vx_dalt = (float)(vx_g_wpin.alt - g_gps->altitude); // in cm's
  // Target nick angle is counter-clockwise from horizon
  // Calculation based on distance and altitude difference
  //vx_target_flightpathangle = atan2(vx_dalt, (vx_g_wpdistance*100.0)); // wpdistance times 100.00 to convert from meters to centimeters
  
  if(vx_g_use_fixed_flightpathangle)
  {
    vx_target_flightpathangle = vx_g_fixed_flightpathangle;
  }
  else
  {
    if (vx_dalt >= 0)//we are to low => pull up
    {
      if(vx_dalt > 2500) //25m to low
      {
        vx_target_flightpathangle = 0.17453; //10°
      }
      else if(vx_dalt > 5000) //50m to low
      {
        vx_target_flightpathangle = 0.34906; //20°
      }
      else
      {
        vx_target_flightpathangle = 0.0;
      }
    }
    else
    {
       if(vx_dalt < 2500) //25m to high
       {
         vx_target_flightpathangle = -0.17453; //10°
       }
       else if(vx_dalt < 5000) //50m to high
       {
         vx_target_flightpathangle = -0.34906; //20°
       }
       else
       {
         vx_target_flightpathangle = 0.0;
       }
    }
  }
    
   
    
  
  
  // *** Barometer controlled altitude
  //vx_g_calculate_baro_altitude();
  //vx_target_flightpathangle = atan( (float)(vx_g_wpin.alt - vx_g_baro_alt) / (vx_g_wpdistance * 100) ); //altitude comes in [cm], wpdistance in[m]
};
//###################### calculate nick end ###########################

//###################### set home begin ###########################
void vx_set_home() //to be called after initialization
{
  /*vx_g_home.lat = g_gps->latitude;
  vx_g_home.lon = g_gps->longitude;
  vx_g_home.alt = g_gps->altitude + 10000;*/
  vx_g_home.lat = vx_g_current_pos.lat;
  vx_g_home.lon = vx_g_current_pos.lon;
  vx_g_home.alt = vx_g_current_pos.alt + 10000;
};
//###################### set home end ###########################


//###################### baro altitude begin ###########################
/*
void vx_g_calculate_baro_altitude()   //FIXME Baro? (post-launch)
{
  //barometer.read();
  //Serial3.println(barometer.get_pressure());
  vx_g_baro_alt = 35000; //(44330 * ( 1 - pow((barometer.get_pressure()/(float)(vx_p0)),0.190294957)) + vx_altitude_correction) *100; //to get long[cm]
};
*/
//###################### baro altitude end ###########################

